#pragma once
#include "planning_base.h"
#include <memory>


/**************状态机*************/
// 挡位
enum Shift {
    m_D, // 前进挡
    m_N, // 空挡
    m_R, // 倒挡
    m_P  // 驻车挡
};

// 转向状态
enum TurnDirection {
    TurnLeft, // 左转
    TurnRight  // 右转
};


/**************车辆基类*************/
class CarBase {
  public:
    // 析构函数
    virtual ~CarBase() = default;
    // 车辆初始化（先摆放默认初始位置，在进行旋转）,中心点坐标，航向角，车辆长和宽
    void InitCar(const double& pos_x, const double& pos_y, const double& heading, const double& length, const double& width);
    // 更新前中点的xy值
    void UpdatePmidf();
    // 更新后中点的xy值
    void UpdatePmidr();
    // 更新几何中心的xy值
    void UpdatePmid();
    // 绘制车辆矩形
    void ShowCar(const COLORREF& color);
    // 绘制轨迹信息
    void ShowCircle();
    // 打印输出
    void CoutInfo();

    // 单帧直行
    void MoveStraightStep();
    // 单帧转向
    void CarTurnStep();

    // 更新四个半径
    void UpdateRinRout(const double& R);
    // 更新转向信息
    void UpdateTurnInfo(const int& turn_state, const double& R);


    // 更新车辆直行信息
    void UpdateStraightInfo();
    // 更新x和y方上的速度和加速度
    void UpdateXYva();
    
  public:
    double car_width = 80.0; // 车辆宽度
    double car_length = 160.0; // 车长

    std::unique_ptr<Point> plf; // 车的左前点
    std::unique_ptr<Point> prf; // 车的右前点
    std::unique_ptr<Point> plr; // 车的左后点
    std::unique_ptr<Point> prr; // 车的右后点
    std::unique_ptr<Point>p_center; // 转向中心点

    std::unique_ptr<Point> pmidf; // 前轴中心
    std::unique_ptr<Point> pmidr; // 后轴中心
    std::unique_ptr<Point> pmid; // 几何中心
    // 以左转弯为例判定内外
    double Rmin = 100.0; // 最小的转弯半径
    double Rof = 0.0; // 外前轮半径
    double Rif = 0.0; // 内前轮半径
    double Rir = 0.0; // 内后轮半径
    double Ror = 0.0; // 外后轮半径

    double R0; // 半斜长，自转半径
    double theta0; // atan(car_length/car_width),其中L是车长，R是车宽

    double speed = 0.0; // 速度，负数代表前进，正数代倒车
    double speed_x = 0.0; // x方向速度，向左为负，向右为正
    double speed_y = 0.0; // y方向速度，向下为正，向上为负
    
    double a = 0.0; 
    double a_x = 0.0;
    double a_y = 0.0;

    double delta_theta = 0.0; // 角速度， 逆时针为正，顺时针为负
    double delta_theta_rot = 0.0; // 自转角速度，逆时针为正，顺时针为负
    double heading_theta = 0.0; // 航向角，为0时表示车辆竖直朝上，向左偏航为负，向右偏航为正

    Shift Gear = m_P; // 挡位

};

/***********************一般车辆*********************/
class CarNormal : public CarBase {
  public:
    CarNormal(const double& pos_x, const double& pos_y,
      const double& heading = 0.0, const double& length = 160.0, 
      const double& width = 80.0);
};